#include <matrix4x4.h>

#include <unittest.h>
#include <debug.h>

/**
 * DESC: Test matrix-4x4 module.
 * IMP: HIGH
 */

void
_assert_equal_matrix4x4( const matrix4x4_t a, const matrix4x4_t b,
			 const double threshold,
			 const char *file, const int line, const char *fname )
{
  unsigned int i, j;
  for ( i=0; i < 4; i++ )
    for ( j=0; j < 4; j++ )
      if ( ! double_is_equal( a.data[ i ][ j ], b.data[ i ][ j ], threshold ) )
	{
	  fprintf( stderr,
		   "%s:%d:%s: Matrix differ at index [ %u ][ %u ]: %e != %e\n",
		   file, line, fname, i, j,
		   a.data[ i ][ j ], b.data[ i ][ j ] );
	  exit( -1 );
	}
}
#define assert_equal_matrix4x4( a, b, threshold ) \
  _assert_equal_matrix4x4( a, b, threshold, __FILE__, __LINE__, __FUNCTION__ )


static const double DOUBLE_PRECISION = 0.000001;

int main( int argc, char *argv[] )
{
  unsigned int i, j;
  matrix4x4_t *identity = matrix4x4_new_identity();

  assert_equal_double( identity->data[ 0 ][ 0 ], 1.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 0 ][ 1 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 0 ][ 2 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 0 ][ 3 ], 0.0, DOUBLE_PRECISION );

  assert_equal_double( identity->data[ 1 ][ 0 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 1 ][ 1 ], 1.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 1 ][ 2 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 1 ][ 3 ], 0.0, DOUBLE_PRECISION );

  assert_equal_double( identity->data[ 2 ][ 0 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 2 ][ 1 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 2 ][ 2 ], 1.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 2 ][ 3 ], 0.0, DOUBLE_PRECISION );

  assert_equal_double( identity->data[ 3 ][ 0 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 3 ][ 1 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 3 ][ 2 ], 0.0, DOUBLE_PRECISION );
  assert_equal_double( identity->data[ 3 ][ 3 ], 1.0, DOUBLE_PRECISION );


  matrix4x4_t *m = matrix4x4_new();
  for ( i=0; i < 4; i++ )
    for ( j=0; j < 4; j++ )
      m->data[ i ][ j ] = i + 100 * j;

  matrix4x4_t *t = matrix4x4_transpose( *m );
  for ( i=0; i < 4; i++ )
    for ( j=0; j < 4; j++ )
      assert_equal_double( t->data[ j ][ i ],  i + 100 * j, DOUBLE_PRECISION );

  matrix4x4_t *a, *b, *c;
  a = matrix4x4_new();
  b = matrix4x4_new();
  c = matrix4x4_new();

  for ( i=0; i < 4; i++ )
    for ( j=0; j < 4; j++ )
      {
	a->data[ i ][ j ] = (i + 1) * (j * 100 + 1);
	b->data[ i ][ j ] = 1 / a->data[ i ][ j ];
      }
  c->data[ 0 ][ 0 ] = \
    a->data[ 0 ][ 0 ] * b->data[ 0 ][ 0 ] +
    a->data[ 0 ][ 1 ] * b->data[ 1 ][ 0 ] +
    a->data[ 0 ][ 2 ] * b->data[ 2 ][ 0 ] +
    a->data[ 0 ][ 3 ] * b->data[ 3 ][ 0 ];
  c->data[ 0 ][ 1 ] = \
    a->data[ 0 ][ 0 ] * b->data[ 0 ][ 1 ] +
    a->data[ 0 ][ 1 ] * b->data[ 1 ][ 1 ] +
    a->data[ 0 ][ 2 ] * b->data[ 2 ][ 1 ] +
    a->data[ 0 ][ 3 ] * b->data[ 3 ][ 1 ];
  c->data[ 0 ][ 2 ] = \
    a->data[ 0 ][ 0 ] * b->data[ 0 ][ 2 ] +
    a->data[ 0 ][ 1 ] * b->data[ 1 ][ 2 ] +
    a->data[ 0 ][ 2 ] * b->data[ 2 ][ 2 ] +
    a->data[ 0 ][ 3 ] * b->data[ 3 ][ 2 ];
  c->data[ 0 ][ 3 ] = \
    a->data[ 0 ][ 0 ] * b->data[ 0 ][ 3 ] +
    a->data[ 0 ][ 1 ] * b->data[ 1 ][ 3 ] +
    a->data[ 0 ][ 2 ] * b->data[ 2 ][ 3 ] +
    a->data[ 0 ][ 3 ] * b->data[ 3 ][ 3 ];

  c->data[ 1 ][ 0 ] = \
    a->data[ 1 ][ 0 ] * b->data[ 0 ][ 0 ] +
    a->data[ 1 ][ 1 ] * b->data[ 1 ][ 0 ] +
    a->data[ 1 ][ 2 ] * b->data[ 2 ][ 0 ] +
    a->data[ 1 ][ 3 ] * b->data[ 3 ][ 0 ];
  c->data[ 1 ][ 1 ] = \
    a->data[ 1 ][ 0 ] * b->data[ 0 ][ 1 ] +
    a->data[ 1 ][ 1 ] * b->data[ 1 ][ 1 ] +
    a->data[ 1 ][ 2 ] * b->data[ 2 ][ 1 ] +
    a->data[ 1 ][ 3 ] * b->data[ 3 ][ 1 ];
  c->data[ 1 ][ 2 ] = \
    a->data[ 1 ][ 0 ] * b->data[ 0 ][ 2 ] +
    a->data[ 1 ][ 1 ] * b->data[ 1 ][ 2 ] +
    a->data[ 1 ][ 2 ] * b->data[ 2 ][ 2 ] +
    a->data[ 1 ][ 3 ] * b->data[ 3 ][ 2 ];
  c->data[ 1 ][ 3 ] = \
    a->data[ 1 ][ 0 ] * b->data[ 0 ][ 3 ] +
    a->data[ 1 ][ 1 ] * b->data[ 1 ][ 3 ] +
    a->data[ 1 ][ 2 ] * b->data[ 2 ][ 3 ] +
    a->data[ 1 ][ 3 ] * b->data[ 3 ][ 3 ];

  c->data[ 2 ][ 0 ] = \
    a->data[ 2 ][ 0 ] * b->data[ 0 ][ 0 ] +
    a->data[ 2 ][ 1 ] * b->data[ 1 ][ 0 ] +
    a->data[ 2 ][ 2 ] * b->data[ 2 ][ 0 ] +
    a->data[ 2 ][ 3 ] * b->data[ 3 ][ 0 ];
  c->data[ 2 ][ 1 ] = \
    a->data[ 2 ][ 0 ] * b->data[ 0 ][ 1 ] +
    a->data[ 2 ][ 1 ] * b->data[ 1 ][ 1 ] +
    a->data[ 2 ][ 2 ] * b->data[ 2 ][ 1 ] +
    a->data[ 2 ][ 3 ] * b->data[ 3 ][ 1 ];
  c->data[ 2 ][ 2 ] = \
    a->data[ 2 ][ 0 ] * b->data[ 0 ][ 2 ] +
    a->data[ 2 ][ 1 ] * b->data[ 1 ][ 2 ] +
    a->data[ 2 ][ 2 ] * b->data[ 2 ][ 2 ] +
    a->data[ 2 ][ 3 ] * b->data[ 3 ][ 2 ];
  c->data[ 2 ][ 3 ] = \
    a->data[ 2 ][ 0 ] * b->data[ 0 ][ 3 ] +
    a->data[ 2 ][ 1 ] * b->data[ 1 ][ 3 ] +
    a->data[ 2 ][ 2 ] * b->data[ 2 ][ 3 ] +
    a->data[ 2 ][ 3 ] * b->data[ 3 ][ 3 ];

  c->data[ 3 ][ 0 ] = \
    a->data[ 3 ][ 0 ] * b->data[ 0 ][ 0 ] +
    a->data[ 3 ][ 1 ] * b->data[ 1 ][ 0 ] +
    a->data[ 3 ][ 2 ] * b->data[ 2 ][ 0 ] +
    a->data[ 3 ][ 3 ] * b->data[ 3 ][ 0 ];
  c->data[ 3 ][ 1 ] = \
    a->data[ 3 ][ 0 ] * b->data[ 0 ][ 1 ] +
    a->data[ 3 ][ 1 ] * b->data[ 1 ][ 1 ] +
    a->data[ 3 ][ 2 ] * b->data[ 2 ][ 1 ] +
    a->data[ 3 ][ 3 ] * b->data[ 3 ][ 1 ];
  c->data[ 3 ][ 2 ] = \
    a->data[ 3 ][ 0 ] * b->data[ 0 ][ 2 ] +
    a->data[ 3 ][ 1 ] * b->data[ 1 ][ 2 ] +
    a->data[ 3 ][ 2 ] * b->data[ 2 ][ 2 ] +
    a->data[ 3 ][ 3 ] * b->data[ 3 ][ 2 ];
  c->data[ 3 ][ 3 ] = \
    a->data[ 3 ][ 0 ] * b->data[ 0 ][ 3 ] +
    a->data[ 3 ][ 1 ] * b->data[ 1 ][ 3 ] +
    a->data[ 3 ][ 2 ] * b->data[ 2 ][ 3 ] +
    a->data[ 3 ][ 3 ] * b->data[ 3 ][ 3 ];

  matrix4x4_t *d = matrix4x4_multiply( *a, *b );
  assert_equal_matrix4x4( *c, *d, DOUBLE_PRECISION );

  matrix4x4_free( a );
  matrix4x4_free( b );
  matrix4x4_free( c );
  matrix4x4_free( d );

  matrix4x4_free( t );
  matrix4x4_free( m );
  matrix4x4_free( identity );

  return 0;
}
